/*
 * Control.h
 *
 *  Created on: Apr 20, 2010
 *      Author: Soheil Damangir
 */

#ifndef CONTROL_H_
#define CONTROL_H_
#include "Motor.h"
#include "xtask.h"

using namespace kthrobot;
namespace Group5
{
struct Occasion{
  bool jLeft;
  bool jRight;
  bool bFront;
  bool bomb;
  bool ball;
  bool any(){return jLeft||jRight||bFront||bomb||ball;}
  Occasion():jLeft(false),jRight(false),bFront(false),bomb(false), ball(false){}
};
class Decision {
public:
  Decision();
  virtual void onDecide(const Occasion&occ, Robot*robot) = 0;
};

class Robot : public XTask{
public:
  Robot(int right_index, int left_index, const Decision&_callback);
  void stop();
  void turn_right();
  void turn_left();
  void u_turn();
  bool move_mm(int d, bool slow);
  /*
   * Robot's velocity in Millimeters/sec
   */
  int speed();
  /*
   * Robot's angular velocity in degree/sec
   */
  int omega();
  /*
   * Distance passed since last reset in Millimeters
   */
  int dist();
  /*
   * Angle change since last reset in Degrees
   */
  int rot();

  void resetPos();
private:
  bool checkOccasion();
  void calc_distances();
  Group5::Motor right, left;
  int right_speed, dist_to_right, right_slope;
  int left_speed, dist_to_left, left_slope;
  int mean_dist, dist_to_front;
  Decision callback;
  Occasion occ;
};
}

#endif /* CONTROL_H_ */
